/*
 * cpp
 *
 *  Created on: 2020年9月2日
 *      Author: 17900
 */
#include "control/control_poshold.hpp"


//Control_posthold
/***************************************************\
功能：
  直升机定点模式
\***************************************************/
void CONTROL_POSHOLD::Pos_NED_Yaw_Command(void)
{
	pos_command_ned[0] = trajectory.cur_target_pos_ned[0];

	pos_command_ned[1] = trajectory.cur_target_pos_ned[1];

	pos_command_ned[2] = trajectory.cur_target_pos_ned[2];

	yaw_command = trajectory.yaw_command;
}

void CONTROL_POSHOLD::Pos_Vel_NED_Feedback(void)
{
	xQueuePeek(queueESKF, &eskf_output, 0);

	pos_feedback_ned[0] = eskf_output.Pos[0];

	pos_feedback_ned[1] = eskf_output.Pos[1];

	pos_feedback_ned[2] = eskf_output.Pos[2];

	vel_feedback_ned[0] = eskf_output.Ned_spd[0];

	vel_feedback_ned[1] = eskf_output.Ned_spd[1];

	vel_feedback_ned[2] = eskf_output.Ned_spd[2];

}

void CONTROL_POSHOLD::Pos_Vel_NED_Loop(void)
{
	#define SPEED_MAX 10
	float vel_command_norm;

	pos_error_ned[0] = pos_command_ned[0] - pos_feedback_ned[0];
	pos_error_ned[1] = pos_command_ned[1] - pos_feedback_ned[1];
	pos_error_ned[2] = pos_command_ned[2] - pos_feedback_ned[2];

	vel_command_ned[0] = pos_error_ned[0] * pidX->Kp;		//X的位置环，PITCH上外环的比例
	vel_command_ned[1] = pos_error_ned[1] * pidY->Kp;		//Y的位置环，ROLL上外环的比例
	vel_command_ned[2] = pos_error_ned[2] * pidZ->Kp;	//Z的位置环，COLL上的比例 2021.12.24

//	vel_target_x += vel_desired[0];
//	vel_target_y += vel_desired[1];

	vel_ned_ff[0] = 0.78f;//0.78f
	vel_ned_ff[1] = 0.78f;
	vel_ned_ff[2] = 0.78f;

	vel_command_ned[0] += vel_ned_ff[0] * trajectory.cur_target_vel_ned[0];
	vel_command_ned[1] += vel_ned_ff[1] * trajectory.cur_target_vel_ned[1];
	vel_command_ned[2] += vel_ned_ff[2] * trajectory.cur_target_vel_ned[2];

	vel_command_norm = sqrt(vel_command_ned[0] *vel_command_ned[0] + vel_command_ned[1] *vel_command_ned[1]);
	if(vel_command_norm > SPEED_MAX)
	{
		vel_command_ned[0] = vel_command_ned[0] * SPEED_MAX /vel_command_norm;
		vel_command_ned[1] = vel_command_ned[1] * SPEED_MAX /vel_command_norm;
	}
}

void CONTROL_POSHOLD::Vel_Acc_NED_Loop(void)
{
	float vel_command_ned_error[3];//,vel_target_y_error,vel_target_z_error;
	float d_vel_command_ned_error[3];//,d_vel_target_y_error,d_vel_target_z_error;
	float accel_feedforward_ned[3];//,accel_feedforward_y,accel_feedforward_z;
	//Get_Vel_Loop_PID();

	xQueuePeek(queueESKF, &eskf_output, 0);
	if(trajectory.trajectory_mod == MinimumSnap){ //minimum snap自带加速度前馈，不需要再利用速度的微分当做加速度的前馈值了
		accel_feedforward_ned[0] = trajectory.cur_target_acc_ned[0];
		accel_feedforward_ned[1] = trajectory.cur_target_acc_ned[1];
		accel_feedforward_ned[2] = (vel_command_ned[2] - vel_command_ned_last[2])/CONTROL_DT;
//		accel_feedforward_ned[2] = 0;
	}
	else{
		accel_feedforward_ned[0] = (vel_command_ned[0] - vel_command_ned_last[0])/CONTROL_DT;
		accel_feedforward_ned[1] = (vel_command_ned[1] - vel_command_ned_last[1])/CONTROL_DT;
		accel_feedforward_ned[2] = (vel_command_ned[2] - vel_command_ned_last[2])/CONTROL_DT;
	}
	vel_command_ned_last[0] = vel_command_ned[0];
	vel_command_ned_last[1] = vel_command_ned[1];
	vel_command_ned_last[2] = vel_command_ned[2];

#define vel_x_alpha   0.557f//0.557f//20hz
	vel_command_ned_error[0] = vel_command_ned[0] - vel_feedback_ned[0];
	d_vel_command_ned_error[0] = (vel_command_ned_error[0] - vel_command_ned_error_old[0]) * vel_x_alpha;
	vel_command_ned_error[0] = vel_command_ned_error_old[0] + d_vel_command_ned_error[0];
	vel_command_ned_error_old[0] = vel_command_ned_error[0];

	vel_intergrate_ned_error[0] = fConstrain(vel_intergrate_ned_error[0] + vel_command_ned_error[0] * CONTROL_DT,Vel_N_limits.IntStateLimits[0],Vel_N_limits.IntStateLimits[1]);

	acc_command_ned[0] = vel_command_ned_error[0] * pidVelX->Kp + vel_intergrate_ned_error[0] * pidVelX->Ki + accel_feedforward_ned[0];		//X速度环

#define vel_y_alpha  0.557f//0.557f//20hz
	vel_command_ned_error[1] = vel_command_ned[1] - vel_feedback_ned[1];
	d_vel_command_ned_error[1] = (vel_command_ned_error[1] - vel_command_ned_error_old[1]) * vel_y_alpha;
	vel_command_ned_error[1] = vel_command_ned_error_old[1] + d_vel_command_ned_error[1];
	vel_command_ned_error_old[1] = vel_command_ned_error[1];

	vel_intergrate_ned_error[1] = fConstrain(vel_intergrate_ned_error[1] + vel_command_ned_error[1] * CONTROL_DT,Vel_E_limits.IntStateLimits[0],Vel_E_limits.IntStateLimits[1]);

	acc_command_ned[1] = vel_command_ned_error[1] * pidVelY->Kp + vel_intergrate_ned_error[1] * pidVelY->Ki + accel_feedforward_ned[1];		//Y速度环

#define vel_z_alpha 0.557f//0.557f//20hz
	vel_command_ned_error[2] = vel_command_ned[2] - vel_feedback_ned[2];
	d_vel_command_ned_error[2] = (vel_command_ned_error[2] - vel_command_ned_error_old[2]) * vel_z_alpha;
	vel_command_ned_error[2] = vel_command_ned_error_old[2] + d_vel_command_ned_error[2];
	vel_command_ned_error_old[2] = vel_command_ned_error[2];

	acc_command_ned[2] = fConstrain(vel_command_ned_error[2] * pidVelZ->Kp + accel_feedforward_ned[2],Vel_D_limits.OutErrorLimits[0],Vel_D_limits.OutErrorLimits[1]);		//Z速度环
	//机头坐标系
	acc_command_body[0] =  acc_command_ned[0]*cos(eskf_output.Attitude[2]) + acc_command_ned[1]*sin(eskf_output.Attitude[2]);

	acc_command_body[1] = -acc_command_ned[0]*sin(eskf_output.Attitude[2]) + acc_command_ned[1]*cos(eskf_output.Attitude[2]);

	acc_command_body[2] = acc_command_ned[2];

}

void CONTROL_POSHOLD::Acc_NE_Roll_Pitch_Loop(void)
{
	float acc_forward,acc_right;
	float acc_norm;
	float acc_command_error[2];//,acc_command_y_error;


	acc_norm = sqrt(acc_command_body[0] * acc_command_body[0] + acc_command_body[1] * acc_command_body[1]);
	if ((acc_norm > G_zinit) && (acc_norm > 0.0f))
	{
		acc_command_body[0] = G_zinit * acc_command_body[0]/acc_norm;
		acc_command_body[1] = G_zinit * acc_command_body[1]/acc_norm;
	}

	float acc_command_x_change = acc_command_body[0] - acc_command_body_last[0];
	float acc_command_y_change = acc_command_body[1] - acc_command_body_last[1];

#define jerk_enable
#ifdef jerk_enable
	float max_delta_accel = CONTROL_DT * 17;
	float acc_target_change_norm = acc_command_x_change * acc_command_x_change + acc_command_y_change * acc_command_y_change;
	if (acc_target_change_norm > max_delta_accel)
	{
		acc_command_x_change = acc_command_x_change * max_delta_accel/acc_target_change_norm;
		acc_command_y_change = acc_command_y_change * max_delta_accel/acc_target_change_norm;
	}
#endif

#define acc_x_alpha 0.557f//20hz
#define acc_y_alpha 0.557f//20hz
	float acc_command_jerk_limited[2];
	acc_command_jerk_limited[0] =  acc_command_body_last[0] + acc_command_x_change;
	acc_command_jerk_limited[1] =  acc_command_body_last[1] + acc_command_y_change;

//	acc_command_body_last[0] = acc_command_jerk_limited[0];
//	acc_command_body_last[1] = acc_command_jerk_limited[1];

	acc_command_error[0] = acc_command_jerk_limited[0] - acc_command_body_last[0];
	acc_command_body[0] = acc_command_body_last[0] + acc_command_error[0] * acc_x_alpha;
	acc_command_body_last[0] = acc_command_body[0];

	acc_command_error[1] = acc_command_jerk_limited[1] - acc_command_body_last[1];
	acc_command_body[1] = acc_command_body_last[1] + acc_command_error[1] * acc_y_alpha;
	acc_command_body_last[1] = acc_command_body[1];

	acc_forward = acc_command_body[0];
	acc_right = acc_command_body[1];
	pitch_command_from_outloop = fConstrain(atan(-acc_forward/G_zinit),pitch_limits.ProErrorLimits[0],pitch_limits.ProErrorLimits[1]);
	float cos_pitch_target=cos(pitch_command_from_outloop);
	roll_command_from_outloop = fConstrain(atan(acc_right*cos_pitch_target/G_zinit),roll_limits.ProErrorLimits[0],roll_limits.ProErrorLimits[1]);
	xQueueOverwrite(queueControlOutputDat, &control_output);
}

void CONTROL_POSHOLD::Altitude_Acc_Loop(void)
{
	#define acc_z_alpha 0.557f
	float acc_command_z_error;
	float d_acc_command_z_error;
	xQueuePeek(queueAccDatFil, &acc_fil, 0);
	xQueuePeek(queueESKF, &eskf_output, 0);
	acc_command_z_error = acc_command_body[2] - (acc_fil.acc_filter[2] + OneG); //	acc_command_z_error = acc_command_body[2] - (acc_fil.acc_filter[2] - eskf_output.Acc_bias[2] + OneG);
	d_acc_command_z_error = (acc_command_z_error - acc_command_d_error_old) * acc_z_alpha;
	acc_command_z_error = acc_command_d_error_old + d_acc_command_z_error;
	acc_command_d_error_old = acc_command_z_error;

	acc_intergrate_d_error = fConstrain(acc_intergrate_d_error + acc_command_z_error * CONTROL_DT,coll_limits.IntStateLimits[0],coll_limits.IntStateLimits[1]);

	output[3] = fConstrain(acc_command_z_error * pidAcc_z->Kp + acc_intergrate_d_error * pidAcc_z->Ki,coll_limits.OutErrorLimits[0],coll_limits.OutErrorLimits[1]);//Z轴上的加速度环

	if(auto_coll_control_mode == 1)
	{
		auto_servo_width[5] = pwm_coll_mid + output[3] * COLL_AMPLIFY;
	}
}


void CONTROL_POSHOLD::PosHold_Control_Step(void)
{
	Pos_NED_Yaw_Command();

	Pos_Vel_NED_Feedback();

	Pos_Vel_NED_Loop();

	Vel_Acc_NED_Loop();

	Acc_NE_Roll_Pitch_Loop();

	Altitude_Acc_Loop();
}

void CONTROL_POSHOLD::Pos_Vel_Loop_Init(void)
{
	acc_intergrate_d_error =0.0f;
	acc_command_d_error_old = 0.0f;
	for(int i=0;i<3;i++)
	{
		vel_intergrate_ned_error[i] =0.0f;
		acc_command_body_last[i] = 0.0f;
		acc_command_body_error_old[i] = 0.0f;
		vel_command_ned_last[i] = 0.0f;
		vel_command_ned_error_old[i] = 0.0f;
	}
}

void CONTROL_POSHOLD::Pos_Hold_Init(void)
{
	pwm_coll_mid = auto_servo_width[5];

	trajectory.Target_Reset();

	trajectory.Trajectory_Reset();

	Pos_Vel_Loop_Init();
}


//void CONTROL_POSHOLD::Trajectory_Target_Receive(void)
//{
//	if(target_receive_flag)
//	{
//		if (false == Target_Queue_Push_Target())
//			Trajectory_Brake_Down_Step(&trajectory);
//		trajectory.first_receive_target ++;
//		target_receive_flag = false;
//	}
//}

void CONTROL_POSHOLD::Heli_Poshold_control(void)
{
	Pos_Loop_count = (Pos_Loop_count+1)>50000? 1000 : Pos_Loop_count+1;//循环次数
	loop_count = Pos_Loop_count;
	//初始化
	if(Pos_Loop_count == 1)	Pos_Hold_Init();
	Flybarless_H1_DecoupingMatrix();//解耦程序,为了采集中位点
	trajectory.Trajectory_Target_Receive();
	trajectory.Trajectory_Step();
	PosHold_Control_Step();
	Attitude_Control_Step();

	Flybarless_CoupingMatrix();//加耦程序
	Flybarless_servos_widths_output();//最终舵机输出
}


void CONTROL_POSHOLD::Set_Limits(void)
{
	N_limits.ProErrorLimits[0] = -1.00f;
	N_limits.ProErrorLimits[1] =  1.00f;
	N_limits.VelErrorLimits[0] = -10.0f;
	N_limits.VelErrorLimits[1] =  10.0f;
	N_limits.IntStateLimits[0] = -0.8f;
	N_limits.IntStateLimits[1] =  0.8f;
	N_limits.OutErrorLimits[0] = -0.50f;
	N_limits.OutErrorLimits[1] =  0.50f;

	E_limits.ProErrorLimits[0] = -1.0f;
	E_limits.ProErrorLimits[1] =  1.0f;
	E_limits.VelErrorLimits[0] = -10.0f;
	E_limits.VelErrorLimits[1] =  10.0f;
	E_limits.IntStateLimits[0] = -0.8f;
	E_limits.IntStateLimits[1] =  0.8f;
	E_limits.OutErrorLimits[0] = -0.461f;
	E_limits.OutErrorLimits[1] =  0.461f;

	D_limits.ProErrorLimits[0] =-5.0f;
	D_limits.ProErrorLimits[1] = 5.0f;
	D_limits.VelErrorLimits[0] = -8.0f;
	D_limits.VelErrorLimits[1] =  8.0f;
	D_limits.IntStateLimits[0] = -2.0f;
	D_limits.IntStateLimits[1] =  2.0f;
	D_limits.OutErrorLimits[0] = -10.0f;
	D_limits.OutErrorLimits[1] =  10.0f;

    Vel_N_limits.ProErrorLimits[0] = -1.00f;
	Vel_N_limits.ProErrorLimits[1] =  1.00f;
	Vel_N_limits.VelErrorLimits[0] = -20.0f;
	Vel_N_limits.VelErrorLimits[1] =  20.0f;
	Vel_N_limits.IntStateLimits[0] = -20.0f;
	Vel_N_limits.IntStateLimits[1] =  20.0f;
	Vel_N_limits.OutErrorLimits[0] = -0.50f;
	Vel_N_limits.OutErrorLimits[1] =  0.50f;

	Vel_E_limits.ProErrorLimits[0] = -1.0f;
	Vel_E_limits.ProErrorLimits[1] =  1.0f;
	Vel_E_limits.VelErrorLimits[0] = -20.0f;
	Vel_E_limits.VelErrorLimits[1] =  20.0f;
	Vel_E_limits.IntStateLimits[0] = -20.0f;
	Vel_E_limits.IntStateLimits[1] =  20.0f;
	Vel_E_limits.OutErrorLimits[0] = -0.461f;
	Vel_E_limits.OutErrorLimits[1] =  0.461f;

	Vel_D_limits.ProErrorLimits[0] = -1.0f;
	Vel_D_limits.ProErrorLimits[1] =  1.0f;
	Vel_D_limits.VelErrorLimits[0] = -10.0f;
	Vel_D_limits.VelErrorLimits[1] =  10.0f;
	Vel_D_limits.IntStateLimits[0] = -3.0f;
	Vel_D_limits.IntStateLimits[1] =  3.0f;
	Vel_D_limits.OutErrorLimits[0] = -2.0f;
	Vel_D_limits.OutErrorLimits[1] =  2.0f;

	roll_limits.ProErrorLimits[0] = -20.0f*D2R;
	roll_limits.ProErrorLimits[1] =  20.0f*D2R;
	roll_limits.VelErrorLimits[0] = -40.0f*D2R;
	roll_limits.VelErrorLimits[1] =  40.0f*D2R;
	roll_limits.IntStateLimits[0] = - 1.2f*D2R;
	roll_limits.IntStateLimits[1] =   1.2f*D2R;
	roll_limits.OutErrorLimits[0] = -150.0f*D2R;
	roll_limits.OutErrorLimits[1] =  150.0f*D2R;

	pitch_limits.ProErrorLimits[0] = -20.0f*D2R;
	pitch_limits.ProErrorLimits[1] =  20.0f*D2R;
	pitch_limits.VelErrorLimits[0] = -40.0f*D2R;
	pitch_limits.VelErrorLimits[1] =  40.0f*D2R;
	pitch_limits.IntStateLimits[0] = - 1.2f*D2R;
	pitch_limits.IntStateLimits[1] =   1.2f*D2R;
	pitch_limits.OutErrorLimits[0] = -150.0f*D2R;
	pitch_limits.OutErrorLimits[1] =  150.0f*D2R;

	yaw_limits.ProErrorLimits[0] = -30.0f*D2R;
	yaw_limits.ProErrorLimits[1] =  30.0f*D2R;
	yaw_limits.VelErrorLimits[0] = -100.0f*D2R;
	yaw_limits.VelErrorLimits[1] =  100.0f*D2R;
	yaw_limits.IntStateLimits[0] = -90.0f*D2R;
	yaw_limits.IntStateLimits[1] =  90.0f*D2R;
	yaw_limits.OutErrorLimits[0] = -30.0f*D2R;
	yaw_limits.OutErrorLimits[1] =  30.0f*D2R;

	coll_limits.ProErrorLimits[0] = -2.0f;
	coll_limits.ProErrorLimits[1] =  2.0f;
	coll_limits.VelErrorLimits[0] = -0.5f;
	coll_limits.VelErrorLimits[1] =  0.5f;
	coll_limits.IntStateLimits[0] = -3.5f;
	coll_limits.IntStateLimits[1] =  3.5f;
	coll_limits.OutErrorLimits[0] = -6.0f;
	coll_limits.OutErrorLimits[1] =  6.0f;

	roll_rate_limits.ProErrorLimits[0] = -150.0f*D2R;
	roll_rate_limits.ProErrorLimits[1] =  150.0f*D2R;
	roll_rate_limits.VelErrorLimits[0] = -100.0f*D2R;
	roll_rate_limits.VelErrorLimits[1] =  100.0f*D2R;
	roll_rate_limits.IntStateLimits[0] = -90.0f*D2R;
	roll_rate_limits.IntStateLimits[1] =  90.0f*D2R;
	roll_rate_limits.OutErrorLimits[0] = -150.0f*D2R;
	roll_rate_limits.OutErrorLimits[1] =  150.0f*D2R;

	pitch_rate_limits.ProErrorLimits[0] = -150.0f*D2R;
	pitch_rate_limits.ProErrorLimits[1] =  150.0f*D2R;
	pitch_rate_limits.VelErrorLimits[0] = -100.0f*D2R;
	pitch_rate_limits.VelErrorLimits[1] =  100.0f*D2R;
	pitch_rate_limits.IntStateLimits[0] = -90.0f*D2R;
	pitch_rate_limits.IntStateLimits[1] =  90.0f*D2R;
	pitch_rate_limits.OutErrorLimits[0] = -150.0f*D2R;
	pitch_rate_limits.OutErrorLimits[1] =  150.0f*D2R;
}
